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Abstract. In this paper we study the optimal formation control of multiple agents whose 
interaction parameters are adjusted upon a cost function consisting of both the control energy and 
the geometrical performance. By optimizing the interaction parameters and by the linear quadratic 
regulation(LQR) controllers, the upper bound of the cost function is minimized. For systems with 
homogeneous agents interconnected over sparse graphs, distributed controllers are proposed that 
inherit the same underlying graph as the one among agents. For the more general case, a relaxed 
optimization problem is considered so as to eliminate the nonlinear constraints. Using the subgradient 
method, interaction parameters among agents are optimized under the constraint of a sparse graph, 
and the optimum of the cost function is a better result than the one when agents interacted only 
through the control channel. Numerical examples are provided to validate the effectiveness of the 
method and to illustrate the geometrical performance of the system. 
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1. Introduction. The study of formations for a group of agents is inspired by 
the behaviors of various animal species in nature. For instance, fish, birds and ants 
always work in a cooperative manner so as to accomplish tasks that are beyond the 
capability of single ones. The formation of agents has its wide range of applications 
both in civilian and military life, where there are generally three basic parts: the deter- 
mination of the underlying graph, the development of cooperative algorithms among 
agents that concerns with an exact assignment and the deployment of controllers that 
are responsible for the stability of the overall system. 

Upon the construction of the underlying communication graphs, [l][24] discovered 
that a geometry with an underlying graph being a rigid one or a persistent one would 
not transform under smooth movements once settled. Meanwhile, other graph proper- 
ties such as connectivity, strong connectivity or even the existence of a spanning tree 
are proved to be sufficient to the convergence of consensus algorithms [TBI [2T] . 

In general, the optimization objectives in a formation system include costs of 
either the relative formation errors [16], the absolute position errors [TTl [19] or the 
minimal traveling distances [25]. On the other hand, [2] discussed the optimal relative 
layout of wheeled robots in terms that the kinetic energy is minimized. [4] focused 
on distributed control design of large-scale dynamically isolated systems using LQR 
optimization. In the trajectory planning of formation reconfiguration, the main con- 
cerns are the minimal executing time [20], the fuel consumption [12] and the kinetic 
energy expenditure|13j . However, nearly all of the studies made the assumptions that 
the local dynamics of agents were pre-specified ones and agents were only coupled 
through the control channels rather than being dynamically related in the open loop. 



"This work is completed during H. Huang's visit at the Australian National University, which is 
supported by The National Natural Science Foundation of China under grant 61074031; C. Yu is 
supported by the Australian Research Council through an APD Fellowship under DP-0877562 and 
subsequently a QEII Fellowship under DP- 110 100538. 

T School of Automation, Beijing Institute of Technology, Beijing 100081, China 
(huanghuangObit . edu . cn) . 

*RSISE, Building 115, The Australian National University, Canberra, ACT, 0200, Australia 
(brad . yu@anu . edu . au) . 



1 



2 



H. HUANG AND C. YU 



In real time applications such as sensor networks, it is always flexible for us to con- 
trol the behavior of each individual agent by designating its interaction rules rather 
than simply setting to zero. Thus problem arises on how to utilize this flexibility on 
interaction parameters so as to improve the performance of a formation system? Here 
by performance we focus on the geometries' deformations with respect to the desired 
one during convergence and the control energy cost. 

Although the problem of parameter optimization is brand new in formation con- 
trol, in recent years, a similar problem has been discussed in the field of consensus 
algorithms as seen in a limited number of literatures. To our best knowledge, the 
optimal weight design over a fixed network topology is addressed in [22] and [23] . The 
optimal weight design under random networks and switching networks are explored 
in [5] and [S] respectively. 

In conventional formation control that concerns formation maintenance while 
moving towards a destination, the discussion is divided into two subsequential steps 
with the first one being the relative formation maintenance or formation stabilization 
irrespective of the destination. For the second step, all agents in formation move 
towards the destination led by one or two agents known as the formation leader. This 
process is always termed destination attainment. During the process, for the purpose 
of formation maintenance, the translational speed of the formation system is bounded 
due to the physical restrictions of each agent. In bearing-only sensor-target localiza- 
tion, when a fleet of UAVs aiming to locate a moving object accurately and as fast 
as possible, they are expected to maintain an optimal geometry[3] during the entire 
process. In such a case, formation maintenance is more important. On the contrary, 
in payload transportation, formation maintenance is not strictly demanded while a 
rapid task execution time is the major concern. 

To this end, in this paper we study the optimal formation control by taking 
both the above two indicators into account simultaneously. With different ratios of 
attentions being paid to those two indicators, we define three kinds of cooperative 
performance and characterize each of them using a quadratic cost matrix. In order 
to find the optimal interaction parameters between agents such that the upper bound 
of the cost function is minimized, the LQR control strategy is considered. Although 
in some literatures distributed optimization was discussed over dynamically isolated 
agents, i.e., agents were interconnected only through the control channel [15l 14]. we 
aim to discover that the optimum in those literatures could be further minimized 
through some carefully selected interaction parameters among agents. 

The remainder of this paper is organized as follows: In Section II, we introduce 
the basic theories on LQR control into the optimal formation control. In Section 
III, a simple system with two agents is considered to illustrate the relationship be- 
tween different kinds of cooperative performance and the cost function we propose. 
In Section IV, the optimization problem with multiple agents is investigated. For 
the special case with homogeneous agents, we propose distributed controllers design 
method by considering dynamically isolated agents. For the more general case with 
heterogeneous agents, in order to avoid the nonlinearity in the constraint conditions, 
a relaxed optimization problem is further proposed and the structured parameter ma- 
trix is calculated based on the subgradient method. In Section V simulations are 
presented to validate those theoretical results and to demonstrate the relationships 
between the cost function and the cooperative performance. Finally conclusions are 
given in Section VI. 
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2. Preliminary. Let Rg denote the set of real symmetric matrices, Rg the set 
of positive definite matrices and Ra the set ofnxn diagonal matrices. The set of 
eigenvalues of matrix A is denoted by X(A) where the largest one is X(A). The n x n 
identity matrix is I n whose zth column is denoted by e, . M is the set of nonsingular 
matrices. Let Aij be the ijth entry, then the ijth block of a matrix A and the ith 
block of a vector v are defined by 



Ay 



A%i— l,2j— 1 An— l,2j 





«2i-l 


,«i = 






«2i 



respectively. We use diag(u), u g R™ or diag([u,]) to denote a diagonal matrix with 
diagonal entries from vector v. Similarly, diag(A) when A £ R nXTl is a vector with 
entries from the diagonal of A. A(A) is a diagonal matrix that has the same diagonal 
entries as matrix A. We have A(A) — diag(diag(A)). The 2-block diagonal of matrix 
A is denoted by A(A) where A(A)f i = A^ and all the other blocks are zeros. 

An undirected graph is denoted by G — (V,E) with |V| vertices and \E\ edges. 
Node i is a neighbor of node j, denoted by i ~ j, if they are connected by an edge in E. 
We do not consider selfloops in a graph. The adjacency matrix of G is A(G) £ rIT' x ' V 
and is determined by 



-4(G), 



i - j 
otherwise 



A Graph G is uniquely determined by an adjacency matrix thus G is also said to be 
generated from A. 

The incidence matrix of G is the \E\ x \V\ matrix H defined by Hij — 1 if vertex 
j is an endpoint of edge i and Hij = otherwise. The oriented incidence matrix H is 
defined by replacing a 1 in each row by a —1. 

For a symmetric matrix A and an adjacency matrix A such that A = A o A 
where o is the Hadamard product, the underlying graph of matrix A is the one that 
is generated from A. 

Consider a system modeled by the standard state-space equation x = Ax + Bu 
where A, B is controllable and the quadratic cost function 

/>oo 

J(x(0),u):= x T (t)Qx(t) +u T (t)u(t)dt (2.1) 



where Q £ Rj and x(0) is the initial state. f^°x T (t)Qx(t) is called the energy 
expenditure of x. Based on LQR control theory, if we consider the state-feedback 
controller 

u(t) = -B T X + x(t) (2.2) 

where X + is the unique positive definite solution to the algebraic Riccati equation 

AX + XA T - XBB T X + Q = (2.3) 

then the cost function J in (|2.1[) achieves its minimum of 

J(x(0)) = a;(0) T A + a;(0) (2.4) 

In order to concentrate on the parameter design issue, throughout the paper it is 
assumed that ||x(0)||2 = 1. For the worst case, J is upper bound by J = X(X + ), and 
minimizing this upper bound of J is the main concern in this paper. 
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However, J is the minimal value of J only if the matrices A, B and Q in the 
ARE (12.31) are determined ones. In a formation system, the interactions between 
neighboring agents, i.e., the offdiagonal entries in the system matrix A, are not any 
intrinsic properties but rather some pending parameters to be designed. Therefore, 
we focus primarily the following optimization problem 

J* = rain sup J {A) (2.5) 

s.t. u = -X + x,\\x(Q)\\ 2 = 1 
A = A Q + A',Aq = diag(A) 

This paper aims to discuss this problem so as to exploit the relationships between the 
interaction parameters A' among agents and the cooperative performance, and to fur- 
ther understand how to design a formation system that meet the desired cooperative 
performance in an optimistic way. 

The following assumption is made true throughout the paper 
Assumption 2.1. The local dynamics of each individual agent is fixed and known 
a priori. 

The system architecture we considered consists of two layers one of which is the 
communication topology among agents as depicted by graph G a . When agents are 
dynamically isolated, G a is an empty graph. In the other layer, the controllers of 
individual agents communicate with one another over an underlying graph denoted 
byG^. 

3. Optimal control of two agents formations. In this section we focus on a 
simple system with two identical agents to illustrate the novel optimization problem 
we consider. More specifically, apart from minimizing the control energy, we also dive 
into the convergence process during destination attainment and distinguish different 
kinds of cooperative performance that meet various tasks requirements. 

We consider two agents on positions xi and x 2 moving towards their destinations 
X\ and x~2 respectively along parallel lines and in a cooperative manner, as shown 
in Fig. 13.11 Generally there are two different cooperative behaviors for the two 
agents one of which is the convergence to the destination of each individual agent, as 
measured by the position error 5xi = Xi — Xi,i — 1,2. The other one is the formation 
maintenance between agents, as represented by the relative formation error of each 
agent i: \Sxi — Sx2\- Three kinds of performance could be considered concerning these 
two behaviors: 

• Destination-first performance: each of the agents concentrates on moving 



towards its own destination in a selfish way. In Fig. 3.1(a) agent 1 left agent 
2 far behind although x\—x\ at t = 1. 

Formation-first performance: each of the agents pays too much attention 
to the other agent and tries to maintain the relative formation on its best 



efforts. In Fig. 3.1(b) agent 1 does not move until agent 2 catches up so that 
\8x\ — 8x 2 \ — 0. 

Neutral performance: each agents find itself a balance between the above 
two kinds of performance, i.e., efforts are put on formation maintenance and 



destination attainment simultaneously, as for example shown in Fig. 3.1(c) 
For the two agents formations, those three kinds of performance could be captured 
by three parameters 71 , 72 and 73 in 

/>oo 

J\ = / 71 (5xi - 8x 2 ) 2 + 72^1 + ^Sxldt (3.1) 
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Fig. 3.1. The three kinds of cooperative performance 



where 71 > corresponds to the importance of formation maintenance and 72 > 
0,73 > correspond to the importance of approaching the destination. Thus when 
71 <C 72 and 71 <C 73, the destination- first performance is captured, and similarly the 
condition that 71 3> 72 and 71 3> 73 correspond to the formation-first performance. 
When 717273 7^ 0, J\ combines both of the two kinds of performance with weights 7j. 
In this case, when Ji reaches a stable value, 8x1 — 8x 2 = 0, which indicates agent 1 
and agent 2 has attained the expected geometry, and 8x\ = 8x2 — 0, which indicates 
the achievement of the destination. 

If we set the entries of the quadratic matrix Q in (|2.1j) as qn = 71 + 72, 912 = 
—71 and Q22 = 71 + 73, J\ then has the compact form of J Q 8x T QSxdt. Thus the 
optimization problem concerning the two-agent formation system could be stated as 

Problem 3.1. Consider two agents with identical local dynamics 



±i(t) = axi(t),i = 1,2 



(3.2) 



and are interconnected through parameters r\,r 2 G K. The overall system is modeled 
by 



S 



±1 




a 


T\ 




X\ 


+ 


Ml 


X 2 _ 




T-2 


a 




X2_ 




U2_ 



:= Ax 



(3.3) 



Assume the quadratic matrix t 
problem 



q 
m 



min||X + || 2 

r, 61 

s.t. 2ax\\ — 
2rixi2 - 
4axi2 - 

X+ = 



x 



11 



2r 2 xi2 - x 12 



x 12 
2xnXi2 



2ax2 



Xn 

X12 



X\2 

X22 



2r 2 x 2 2 



> 0. Find the solution to the optimization 



+ 9 = 
+ q = 
2x 12 x 2 2 



2r\X\ 



2m = 



(3.4) 
(3.5) 
(3.6) 



Remark 3.1. When Q is a positive definite matrix and when B = I , the existence 
of the positive solution X + to the optimization problem is guaranteed. 

The solution to the above optimization problem is discussed in details in Section 
21 Here we use a numerical example to show that the optimal solution is obtained 
when fx 7^ and r2 7^ 0. 
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Example 3.1. Consider the cost matrix 

2 -1 
-1 2 

According to (|3.ip . Q indicates that the maintenance of formation is as important as 
the convergence to the destination. 

The local dynamics of each agent is uniquely chosen as a single integral with a = 2 . 
First we consider agents with no interactions, i.e., A\ — 2diag(l) and n = r2 = 0. 
By solving Problem \3.1l one can obtain the optimal upper bound of the cost value 
J — 4.65. 

On the other hand, if interconnections between agents are considered, for example 



A 2 = 



2 1.2 
-1 2 



the 2-norm of the solution X + is X(X + ) — 4.47, which is better than the previous 
one. The corresponding closed-loop system matrix is 



A.-X- 



-2.46 1.21 
-0.98 -2.44 



Conjecture 3.1. If the local dynamics of agents are set to be Sii(t) = 25xi{t), i = 
1,2, for the worst case, the system with interaction Ai has better formation perfor- 
mance and consumes less control energy than the situation when the two agents are 
disjoint. 

4. Optimal Formation Control of Multiple Agents. For a group of agents 
that is assigned with cooperative payload transport tasks, each agent aims to approach 
its destination while maintaining a certain geometry during the process. In most cases, 
one may have to choose either the destination-first performance which may result in 
formation failure during the convergence, or the formation-first performance which 
may increase the task execution time. Similar issues arise in other situations such as 
formation reconfiguration and obstacle avoidance. Thus a balance between those two 
kinds of performance is demanding and is worth of exploration. 

Consider a set C = {Q € : qa > 0, qij < 0}. Then the three kinds of 
performance introduced in Section [3] could be represented by choosing appropriate 
entices of Q € C. 

Under Assumption 12.11 we decouple the matrix into 

A = A Q + A' 

where Ao = A(A) if the dynamics of each agent is in two dimensions and Ao = A(A) 
if it is in one dimension. Specially, when A' = 0, all agents are dynamically isolated 
with each other in the open loop. 

Based on the LQR control, when focusing on the worst case, the optimization 
problem is formalized as 

Problem 4.1. 

min \\X\\ 2 

A' 

s.t. A T X + XA - XB T BX + Q = (4.1) 
A = A Q + A', A = A(A), (A, B) controllable 
QeC, X e Rt 
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(a) Congruent underlying graph for system (b) Different underlying graphs for system 
with homogeneous agents with heterogeneous agents 

Fig. 4.1. The underlying graphs of the two layers in a formation system. 



When A' solves the optimization problem, (A, Q) is called a matched pair and the 
corresponding positive solution of the ARE (|4.ip is denoted by X*(A,Q) which is 
sometimes abbreviated by X*. 

Following are two well-proved lemmas from |14j that motivate our research. 

Lemma 4.1. Consider the algebraic Riccati equation (|4.1I) . The unitary sta- 
ble hermitian solution X + is a continuous function of the 3-tuple (A, Q, B T B) G $ 
where $ : {(A, Q, B T B)\Q e R^ , (A, B) stabilizable} . When A, B, Q are continuous 
functions of a vector 7, so does X + . 

Lemma 4.2. Let n e (A,Q,B T B) = {(A,Q,B T B)\\\A- A\\ + ||Q-Q|| + \\B T B- 
B T B\\ < e}. For every (A,Q,B T B) € $ there exist positive constants K and e such 
that 



\\X+(A,Q,B T B)-X+{A,Q,B T B)\\ < 
K(\\A -A\\ + \\Q- Q\\ + \\B T B - B T B\\) 1 ' 2 



(4.2) 



is satisfied for all (A,Q, B T B) £ n e (A,Q, B). X + is said to be a locally Lipschitz 
continuous function of order 1/2 . These properties of the solution X + guarantee the 
feasibility of the optimization problem 14.11 

Recall Problem 14.11 although the objective function is convex, the constraints 
of the ARE (|4.1[) on arguments X + and are nonlinear, and the variable X + is 
expected to be symmetric, which falls into the category of the nonlinear semidefinite 
programming(NSDP) problem. Meanwhile, this minimax problem is nonsmooth on 
X + , thus most of the analytical optimization techniques are inapplicable. Although 
the nonlinear optimization toolbox in Matlab may sometimes give structured solu- 
tions on both A' and X + , generally special cares are required so as to accelerate the 
optimization process and it may sometimes fail. 

On the other hand, even if there are some analytical iterative methods to solve 
the optimization problem 14. 1[ the optimal solution of X + and A' are dense matrices 
which indicates that the underlying graphs G a and Gp are complete graphs. This 
is obviously not applicative especially for large-scale formation systems. In the re- 
maining context, we will first consider a special case where agents are homogeneous 
ones. Distributed controllers are considered that inherit the communication graph G a 



among agents: G a — Gp, as for example shown in Fig. 4.1(a) Further for the general 



situation, we discuss the optimization problem with topology constraints imposed in 
the agents layer. Examples of the underlying graphs G a and Gp are illustrated in 
Fig. 



4.1(b) 



4.1. Distributed Controllers Design for Homogeneous Agents. As men- 
tioned in the preliminary, the formation system is broken down into two layers each 
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of which has its own communication topology. In practical scenarios, agents and con- 
trollers always share communication channels, therefore generally G a — Gp is more 
preferred. In this subsection, we consider formation systems consist of homogeneous 
agents and let G a = Gp = Gh- The following assumptions are also made 
Assumption 4.1. In a formation system 

• the dynamics of each agents is decoupled along x and y axis such that we can 
focus on the one- dimensional model of each agent 

• the local dynamics of agents are congruent: 

• the interaction parameters among any pairs of neighboring agents are con- 
gruent: 

ii = axi + (4-3) 

• the input matrix B is an identical matrix. This assumption is realistic and is 
easy to be satisfied in a formation system 

• the quadratic matrix Q has identical diagonal entries and identical nondiag- 
onal entries, i.e., 

diag(Q) = gl, Q - A{Q) = pA 

where A is the adjacency matrix of the assigned graph Gh ■ 
Assumption 14.11 allows us to decouple the overall formation system into n inde- 
pendent subsystems, and thus the design of distributed controllers is also broken down 
accordingly, which leads to the following controller design method. 

Theorem 4.3. Consider system (|4.3[) and the Assumption \4-l\ The optimal 
solution X* to Problem \4-.l\ is 

X* = xl + yA (4.4) 

where 



x = a+Va*+ q , y=^ Ta 

and is the optimal controller when when b ~ 2x -4a • 

Proof. Under Assumption 14.11 the system matrix A and quadratic matrix Q has 
the special form of 

A = al + bA, Q = ql +pA (4.5) 

Assume the optimal solution has the form X* = xl + yA such that the ARE is 
rewritten as 

(al + bA){xI + yA) + {xl + yA){aI + bA) - {xl + yA) 2 + {ql + pA) = 
which has the equivalent expression of 



2{axl + bxA + ayA + by A 2 ) - {x 2 1 + 2xyA + y 2 A 2 ) + {ql + pA) = (4.6) 
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Let P be the orthogonal permutation matrix of A such that 

A A = P T AP 

where the diagonal entries are the eigenvalues of the adjacency matrix A{G). Left 
and right multiplying the left hand side expression in equation (|4.6[) by P T and P 
yields 

(2ax - x 2 + q)I + (2bx + 2ay - 2xy + p)A A + (2by - y 2 )A 2 A = (4.7) 

Due to the feasibility of the ARE and the positive semidefinite constraints on the 
solution X* , the parameters x,y,b are 



-2a - ^4a 2 + 4q 



b= P 



2x - 4a 

V = 2& (4.8) 

Substituting b by y/2 back into equation (|4.7[) and with some trivial calculations, we 
obtain that 

(2a -x)(x + \ l (A)y) +q + X l (A) P = (4.9) 



Due to the expression of x in (I4.8[) . 2a — x < 0. As the quadratic matrix Q > 0, it is 
also true that q + Xi(A)p > for all i. Thus equation (|4.9|) is satisfied if and only if 

xI + yAe K+ 

This infers that X* with x and y given in (|4.8[) is the positive definite solution and is 
the optimal controller when b = 2x P -4a ■ ^ 

Remark 4.1. Controller K = —X* is the stabilization controller with underly- 
ing graph generated from A. Thus the communication structures among agents and 



controllers are congruent ones, i.e., G a — Gp as, for example, the one in Fig. 4.1(a) 



Remark 4.2. The synthesis of controller for each agent is carried out in a dis- 
tributed way and the control signal passed to each agent is generated using information 
from the neighbors. This property provides the formation system the feature of scala- 
bility. 

In [J] the authors focused themselves on the scalable synthesis of distributed 
controllers for large-scale identical isolated subsystems, i.e. A 1 = 0, and further 
explored the relationship between the local controllers and the stability of the overall 
system. Next we will show that, compared to the formation system where A 1 = 0, a 
set of carefully designed interaction parameters would indeed give the overall system 
a better cooperative performance and require less control energy. 

Corollary 4.4. Consider a formation system consists of N agents under As- 
sumption \4-l\ For a matched pair (Aq + A' , Q), A' = if and only if Q € M.\. 

Proof. Recall (|4.7I) , if p = 0, due to the positiveness of q, it is necessarily that 
y = and 6 = 0, which indicate A' = 0. According to the solutions x,y and b in 
Theorem 14.31 when b = 0, q — 0, which proves the sufficient condition. □ 

When qij = 0,i ^= j, it means no cooperative behavior is expected in a formation 
system. Corollary 14.41 indicates that in such cases system with disjoint agents can 
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perform better. However, due to the inverse negative proposition of Corollary 14.41 
as long as there exists g^- ^ 0, having agents communicate with others at some 
appropriate parameters in the open loop is a better choice. Note that these results 
only apply to system with identical agents and with equivalent weights being put on 
the diagonal of the cost matrix Q. Systems with heterogeneous agents are not eligible 
to this property. Actually, when an is not identical, interactions are required even if 
qtj = so as to make up a matched pair. 

For a set of homogeneous agents, by finding the appropriate parameter b between 
each pair of neighboring agents, we developed the optimal state-feedback controllers 
that inherits exactly the same underlying structure as the agents, and the upper 
bounds of the quadratic cost function of the formation system is minimized. 

4.2. Heterogeneous Formation Systems under Structure Constraints. 

In this subsection, we further consider a more general case where all those restrictions 
in Assumption 14. II no longer hold true. When the dynamics of agents is coupled along 
x axis and y axis, the dimension of the overall system model is 2n instead of n and 
thus the local dynamics as well as the interaction parameters between neighbors are 
captured by 2 x 2 matrices. In order to find optimal interaction parameters for a 
formation system where agents communicate over an assigned graph G, we define a 
set of structured matrices whose underlying graph is G: 

T G = {M £ M 2 " x2 " : Mfj = M fl = AjM fj ,i ? j} 

where A is the adjacency matrix of G. 

The state-space model of the overall system is 

p = Ap + Bu (4.10) 

where A £ Tq D Ms and B £ Tq n M. Note that for the general case, we restrict our- 
selves to systems where interactions among agents are independent of the transmission 
direction, i.e., matrix A is symmetric. This is only for the purpose of simplicity and 
all the results bellow apply to nonsymmetric case as well. Meanwhile, the structure of 
the input matrix indicates that agents exchanges their states and their control inputs 
simultaneously with the neighbors. 

As mentioned before, the most difficult part when dealing with Problem 14.11 is 
the nonlinear constraints in the ARE. This inspires us to exploit methodologies to 
eliminate this restriction. 

What we focus on in this paper is the two norm of X + . We noticed that the 
upper bound of the positive solution to the ARE (|4.1|) has been discussed widely by 
researchers in the related fields. Quite a few upper bounds are given in literatures 
among which the following result borrowed from [101 is known as a relatively tighter 
bound than many others. 

LEMMA 4.5. Consider ARE (|4.1j) . The norm of the positive semidefinite solution 
X + is upper bounded by 

\\X+\\ < \\P\\^ 2 \\APA + QW 1 ' 2 + fi(AP) (4.11) 

where (jl(AP) = 1/2A(AP + P T A) and P = (B T B)- 1 . 

Remark 4.3. This upper bound o/||A + || also validates the fact that the minimal 
value of J is correlated with A and Q, and when Q and B are not diagonal matrices, 
the minimal upper bound of ||A + | is the one when A is nondiagonal as well. 
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We then propose the following relaxed optimization problem that is parallel to 
Problem 14.11 but the nonlinear constraint is avoided. Note that as we consider for- 
mation on a plane, the local dynamics between each pair of agents is represented by 
a square matrix A'-.. 

Problem 4.2. 

mm(f>(A') = \\P\\ 1/2 \\APA + Q\\ 1/2 + ^\(AP + PA) (4.12) 

s.t. A = A a + A', A = A(A), AeT G n R s , 
QeT G nc, b g Tq n M, P = (b t b)- 1 

The minimum of 4>(A') is calculated when the the argument A' fits into the desired 
underlying graph G. This minimum could be further reduced when this structure 
restriction is abandoned, which is, however, accompanied by the increase on the com- 
munication cost. 

Theorem 4.6. The optimization problem (14. 12)) is convex. 

Proof. It is well-known that X(A) is a convex function on A. Due to the posi- 
tiveness and symmetry of APA + Q, \\APA + Q\\ = \(APA + Q). The conclusion is 
then self-evident. □ Despite of the convexity, the function <fi(A') is non-smooth and 
thus is non-differential, which requires some special optimization techniques. In our 
case, the constraints are linear matrices constraints, thus it is then an ordinary linear 
programming(LP) problem. Interior-point methods and subgradicnt methods are two 
of the frequently used techniques when dealing with LP, as in [8j EJ [22] . More 
specifically, a similar problem is discussed in [5] where the objective function 4> is 
the expectation of the system matrix. Due to this similarity, here we also adopt the 
subgradient algorithm as in 8 to deal with this structure constrained optimization 
problem. 

Matrix A is actually an affine function of the interaction parameters Aij,i ~ j. 
Assume the underlying graph G has m edges indexed from 1 to to. Define a vector 
x e M 4m that is partitioned into m sub- vectors Xk = [x\ x\ x\ xf\ , k G [1, to]. 
The entries ij,p6 [1,4] indicate the interactions between agent i and agent j that 
are connected by edge I: 

= Afj 

Then the affine form of A on vector x is 

m 

a(x) = a + £ (44+44+44+44.) (4 . 13) 

k=l,j>i 

where 

T T 

e2i-ie 2j _i + e-2,j-\e % i-\ 
e 2i -iel 3 + e 2o el l _ 1 

e 2 ie^- + e 2 jeli 



4 = 
4 = 
4- 
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The partial differential of A'(x) or A(x) with respect to x\ is 

dA(x 



and further 



dx P "%P6 {1,2,3,4} (4.14) 



i(^) := ^^ P +Q) = ^§ + EfjP^P e {1, 2, 3, 4} (4.15) 



■-fc 

The Hessian of </>(j4') is then 



fl^l™ = Aft, ^ » + -« ^ « 

= Mv T L{x p k )v + ^u T {Ef.P + PEf.)u (4.16) 



where M = — . v . Vector v is the unit eigenvectors associated with the largest 

eigenvalue of APA+Q and u is the one associated with X(AP+PA). The optimization 
algorithm is then carried out by computing the sub-gradient of <fi(A k ) at A k on each 
step with stepsize 7 fc > 0. The stepsize satisfies the diminishing rule: 



lim 7 fc = 0,y^7 fc = oo 



k— >oo 



As pointed out in 22 , for the convex optimization problem, the convergence of the 
algorithm is well-known. 



Algorithm 1: Optimal Parameters Design 

Initialize A' 1 eT G , B G T G and Q G T G H C, A, i = 1, e > 0; 
Repeat; 

Compute M; 
Compute and u z ; 
Compute H z at a; 4 ; 
Set A H+1 = A' 1 - YH l ; 
i:=i + l; 
Until |A0(A')| < e; 



The optimal parameters generated from Algorithm [T] minimize the upper bound 
of || A* || in the ARE (|4.ip . We point out that there indeed have certain cases where a 
lower upper bound may relate to a higher value of || X*\\ . However the original Problem 
4.11 is a great trouble while this relaxed optimization problem could be carried out 
using the reliable subgradient method. Meanwhile, in most of cases we experimented, 
lower upper bound of ||A*|| always corresponds to a smaller value of ||A*||. 

Substituting the structured matrix A generated from Algorithm [T] into the ARE 
(|4.ip . the resulting LQR controller K = —B T X* is possibly a dense matrix without 
any structure constraints because of the density of X*. In general, X* is usually a 
matrix with nonzero entries even when A is a sparse matrix, as for example the ones 



in Fig. 4.1(b) This result is consistent to [15] where when no communication ex- 



penditure was considered, the undirected underlying graph of the optimal interaction 
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(a) G a (b) G b 

Fig. 5.1. Two graphs with six vertexes 



graph among the LQR controllers was typically a complete graph. This reveals that 
a system with distributed controllers consumes less energy compared with a decen- 
tralized one. However a complex topology is obviously not practical, especially when 
the scale of the system grows. In the above research, due to the uniqueness of the 
positive solution of the ARE (14. ip . it is impossible to find any structured solution so 
as to reduce the communication burden among controllers. Other methodologies are 
required to deal with this deficiency, which is our ongoing works. 

In this section, the optimization problem is built up with the objective func- 
tion being nonsmooth and with nonlinear constraints. Structure restrictions on the 
underlying graphs are introduced for the purpose of realtime applications. To find 
an analytical solution to the optimization problem, a special formation system with 
homogeneous agents is considered. The system is decoupled into n independent sub- 
systems and the desired underlying graphs are imposed both among agents and among 
controllers. For the more general relaxed optimization problem is proposed 

that eliminates the nonlinear constraints and thus is solved based on the subgradicnt 
method. The underlying graph G a is designed to be the desired one. 

5. Examples. The proposed algorithms are validated on systems with identi- 
cal agents and systems with heterogeneous agents respectively. In order to observe 
the cooperative performance of the relative formations during convergence more intu- 
itively, we consider the relative formation system in the edge space that is transformed 
through 

e = {H®I 2 )p 

where H is the oriented incidence matrix of a rigid graph G = (V, E) and p is 
the coordinates of agents. For the desired formation pd € M. 2n of n agents and the 
corresponding e d e K 21 ^ 1 , we define a relative formation error function with respect 
to ed as 



\ e i\\ \\ e d t 



I ex || ||e dl || 



(5.1) 



When fe = 0, the geometry formed by the agents coincide with the desired geometry 
although their actual coordinates may differ. A larger relative formation error corre- 
sponds to greater deformation effects of a geometry with respect to the desired one. 



5.1. Formations with homogeneous agents. For a formation system with 
identical agents and identical interaction parameters, Theorem 14.31 provides an ana- 
lytical method to design distributed controllers so as to minimize the cost function 
under the worst case. 
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$ final position 
O initial position 




Fig. 5.2. Formations of six agents 




(a) t = O.i 




(b) t = 1.48s (c) t = 2.28s 

Fig. 5.3. Snapshots of the six agents 



(d) t = 3.08s 



The underlying graph is the one in Fig. 5.1(a) The parameters in (I4.5[) are 
a = 2, q = 8 and p = — 2. According to Theorem l4.31 the distributed controller X* has 
identical diagonal entries of 5.46 and nondiagonal entries of -1.37. The trajectories 
of the six agents are shown in Fig. 15.21 with snapshots in Fig. 15.31 Fig. 5.4(a) 
demonstrates the position errors of the six agents with respect to the desired positions. 
It takes approximately 8 seconds for them to achieve the desired positions. However, 
according to Fig 



5.4(b) 



at approximately t 2sec, the relative formation error 
converges to zero, which indicates that with high attentions being paid to the relative 
formations of the six agents, they attain the desired geometry before arriving at the 
destination. 

In order to observe different kinds of cooperative performance, the six agents are 
initialized at the same spot as shown in Fig. 15.51 When q = 8,p = —2, relative forma- 
tion is more important during the assignment, thus agents spread immediately after 
they set off and maintain the desired geometry when approaching the destination. On 
the other hand, if the destination-first performance is more impotent in the mission, 
we set q = 3 and p = —0.2. Then the six agents approach their own destinations 
radially and form the desired geometry at almost the same time as they achieve the 
destinations at t « 3sec. The formation errors for the two kinds of performance are 
shown in Fig. I5.6l in blue solid lines and red dashed lines respectively. This infers that 
the parameters in the cost matrix Q allow us to take into account the geometrical 
performance during convergence. 

5.2. Formations with heterogeneous agents. When agents exhibit different 
local dynamics and when the interactions between neighboring agents various from one 
another, Algorithm[T]is proposed to solve for the set of optimal interaction parameters. 

We consider six agents in a formation system that communicate over graph Gb 



as shown in Fig. 5.1(b) 
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(a) Position errors (b) Formation errors 



Fig. 5.4. Position errors and formation errors of the six agents 




(a)g = 8,p=-2 (b) q = 3,p = -0.2 

Fig. 5.5. Formation-first performance and the destination-first performance 



The local dynamics of agents are randomly chosen as 

A _ [-8.12-2.34] A _ [-2.75 -5.94] a _ [ 5.83 -3.51] 
711 — L-2.34 -4.38 J ~~ L -5.94 1.09 J ' ^3 — [ -3.51 3.76 J 

A _ r 9.55 -3.22 ] a _ [-0.50 -3.84] a _ \ 3.48 -2.09] (r 9 \ 

^4 — L -3.22 -11.30 J i-^S — L-3.84 -7.07J '^6 — [ -2.09 -5.20 J 

and the nonzero input matrices are 

td_ _ T3.99 ] R_ _ T-0.15 ] r_ _ T3.53 ] r_ _ f 1.06 
— L -3.67 J J ^12 — L 0.19 J '^13— L -1.33 J > -°16 — L ( 



-0.15. 



r_ _ T-3.20 ] r_ _ T 2.89 ] r_ _ [ —2.46 ] 
^22 - L 4.41 J '-°23 — L -1.54 J > ^26 ~ [ Q 0.41 J ' 

R [-4.69 ] d [ 1.57 ] r [1.49 ] 

-°33 — L -2.32 J >-°34 — [ -0.63 J ' -°35 — [ -0.94 J 



R_ _ [-3.22 ] d _ [-1.23 ] r_ _ T0.26 ] 
^44 — L -1.12 J i -°45 — L 2. 98 J '^46 — L -1.31 J' 

R_ _ [-3.45 ] r_ _ [0.90 1 
-°55 — L 4.49J>- D 56 — I 0.08 J' 

B 6 - e =[-Y 5 -len} (5-3) 
The quadratic matrix is 



)_ _ [13.50 1 n r-1.17 ] n [-1.74 ] n _ _ [-4.21 " 

511 — I 22.50J i Wl2 — L -2.63 J ' ^13 — L -1.48 J J Vie — [ -3.12. 

4.77 ] 
-5.31 J > 



Q22 — [ 13 0° 22°50 ] ) Q23 — [ O 48 -2°59 ] ) ^26 — [ 

Q33 = [ O 8 30 ] ' Q$i — [ O 97 -0°89] ' Q$5 = [ O 77 -1I24] ' 

Cl- - [ 13.50 1 r> [-4.84 ] n [-4.01 ] 

^44 — [ 22.50 J > V45 — |_ —5.17 J ' ^46 L -5.23 J 

n- - [is 1 r>- — r-2-67 o ] 

^55 — [ 30 J ; V56 — L -2.29 J ) 



) - — F 13.50 1 
S66 — [ 22.50 J 



(5.4) 



and all the other blocks are zeros. Using Algorithm [TJ the subgradient matrix con- 
verges to zero after approximately 63 times of iterations. The interaction parameters 
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Fig. 5.6. Formation errors for different kinds of cooperative performance 



"0 5 10 15 20 25 30 35 40 45 50 

Samples 



Fig. 5.7. Worst-case cost values of six- agent formations. The bottom dashed line is the opti- 
mized cost value J* 



in matrix A' are 



A'_ _ r 4.79 3.60 1 Al _ I" -3.30 3.431 A' _ [2.68 -2.72" 
A 12 — L —1.12 -0.96J 1^13 — L 6.41 2.21 J '^16 — Lo.l 



L34 -0.57 J 



A'_ _ [ -3.23 1.971 A>_ _ [ 2.31 -1.82 
^23 ~~ L 10.33 0.61 J >^26 ~~ L " 



0.96 -4.78. 

3.86 -0.231 if _ [-2.14 0.69" 



A'_ — r -3.86 -0.231 Al _ r- 
^34 — L -0.10 1.07 J ' ^35 — L -1.99 0.56 J ' 

A'_ _ r 3.65 0.63 1 A'_ — r-0. 38 -2.401 
^45 — L-0.38 -3.88 J '^46 — L -0.35 -2.73 J > 

A' - [-0.54 3.93] /c x\ 

^56 — L 0.14 3.23J ' 

and all the other blocks are zeros. According to the structure of matrix A', the 
underlying graph is exactly the one in Fig. 5.1(b)| and the cost function achieves the 



minimum of J* = 3.39 for the worst case. We tested 50 samples of randomly selected 
A' under the structure constraints. Greater cost values are observed on all of those 
samples. The data are recorded in Fig. 15.71 The dashed line at the bottom is the 
value of ||X*|| = 3.39 and all the blue stars are different worst-case cost values J when 
A' € Tq changes. This validates the effectiveness of the subgradient algorithm. Fig. 
5.81 is an example of formations of the six agents under the matched pair (_o Q)- 
Snapshots during the process are shown in Fig. 15.91 

For comparison, we consider a formation system where agents are dynamically 
isolated. The local dynamics, the input matrix and the cost matrix are congruent to 
the ones in (|5.2p . (|5.3[) and (15.41) respectively. The optimal controllers are calculated 
by the ARE (14. ip . For the worst case, the cost function has a minimal value of 
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Fig. 5.8. Formations of six agents 




(a) t = Os (b) t = 0.22s (c) t = 0.66s (d) t = 1.23s 

Fig. 5.9. Snapshots of the six agents during attainment 



J = 4.53, as shown in sample number 50 in Fig. 15.71 We compare the performance of 
a system with isolated agents and a system with optimized couplings (|5.5I) . For both 
the situations, agents are initialized at the worst case, i.e., po = — 3u with v being 
the orthogonal eigenvector of A(A ) and X(A + A') respectively. The two plots in 
Fig. 15.101 show their trajectories during the stabilization processes. The formation 
errors for system under the matched pair (Ao + A' , Q) and the pair (Aq, Q) are shown 
in Fig. 5.11(a) For the two situations, the snapshots of the geometries at time slot 
t = 0.21sec are shown in Fig. 5.11(b) According to Fig. 15. Ill the geometry of agents 
under the matched pair is more similar to the desired geometry at the same time slot. 
Formation systems with isolated agents have relatively greater value of cost function, 
and thus its relative formation errors are consistently greater than the one with agents 
interacted over optimal parameters. 



6. Conclusions. In this paper, we proposed an optimal LQR control strategy 
for a group of agents to maintain desired geometries while moving towards the des- 
tination. Upon the three kinds of cooperative performance that were characterized 
by the cost matrix, it was proved that, compared to system with agents communi- 
cated only through the control channel, the upper bound of the minimum cost value 
could be further reduced by adjusting the interaction parameters between the pairs 
of neighboring agents and by the LQR controllers. Distributed controllers that inher- 
ited the desired underlying graph were developed for the set of homogeneous agents. 
When agents were heterogenous ones, the optimization problem was relaxed so as to 
avoid the nonsmoothness and nonlinearity in the constraints. Under the constraint 
of a sparse underlying graph, parameters among agents were generated by a reliable 
iterative algorithm. Numerical examples further illustrated the relationship between 
the cost function and the cooperative performance we considered. 
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(a) Optimized couplings (b) Isolated agents 



Fig. 5.10. Trajectories of six agents being initialized at their worst cases. 




(a) Formation errors (b) Snapshots at t = 0.21sec 



Fig. 5.11. Formation errors and the snapshots of systems with isolated agents and optimal 
coupled agents 
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